package kGeom;
import java.net.*;
import java.util.EmptyStackException;
import java.io.*;


public class kPointCloud extends kElement {
	private kVec[]	points;
	private kRange	range;
	protected float	thresh;
	private static float	defaultThresh = 0.00001f;

	/*
	 * CONSTRUCTORS
	 */

	public kPointCloud() {
		this(new kVec[0], 0.00001f);
	}
	public kPointCloud(kVec[] pArr) {
		this(pArr,defaultThresh);
	}

	/**
	 * REFERENCE CONSTRUCTOR
	 * 
	 * @param pArr
	 *            initial point cloud
	 * @param t
	 *            threshold value
	 */
	public kPointCloud(kVec[] pArr, float t) {
		points = pArr;
		thresh = t;
		updateRange();
	}
	
	/**
	 * Constructs a point cloud from a space-separated string
	 * example: "x,y,z x,y,z x,y,z x,y,z"
	 * example: "(x,y,z) (x,y,z) (x,y,z) (x,y,z)"
	 * example: "[x,y,z] [x,y,z] [x,y,z] [x,y,z]"
	 * 
	 * @param str
	 *            string
	 */
	public kPointCloud(String str){
		this();
		try {
			String []splits = str.split(" ");
			if (splits.length == 0){throw new EmptyStackException();}
			for (int n = 0; n < splits.length; n++) {
				append(new kVec(splits[n]));
			}
		} catch (Exception e) {
			System.out.println("!!!!! kPointCloud(String str): "+e.toString());
		}
	}	

	/*
	 * METHODS
	 */

	// TODO: get and set threshold and eliminate duplicates accordingly

	public int count() {
		return points.length;
	}

	/*
	 * POINT MANAGEMENT METHODS
	 */

	public kVec[] getPts() {
		kVec[] returnPts = new kVec[points.length];
		for (int n = 0; n < points.length; n++) {
			returnPts[n] = points[n];
		}
		return returnPts;
	}
	
	public kRange getRange() {
		return kRange.clone(range);
	}

	public kVec getAt(int index) {
		if ((index >= 0) && (index < points.length )) return kVec.clone(points[index]);
		else {
			System.out.println("!!! kPointCloud.getAt("+index+") index out of range");
			return new kVec();
		}
	}

	public boolean setAt(int index, kVec p) {
		if ((index >= 0) && (index < points.length)) {
			points[index] = p;
			range.expandToPoint(p);
			return true;
		} else {
			System.out.println("!!! kPointCloud.setAt("+index+") index out of range");
			return false;
		}
	}

	public boolean removeAt(int index) {
		// TODO: update the range based on this removal... this ruins the range's validity!!!!!!!!!!!!
		if ((index >= 0) && (index < points.length)) {
			kVec deleted = kVec.clone(points[index]);
			kVec[] tempPts = new kVec[points.length - 1];
			int m = 0;
			for (int n = 0; n < points.length; n++) {
				if (n != index) {
					tempPts[m] = points[n];
					m++;
				}
			}
			points = tempPts;
			
			if (  (deleted.x == range.maxX())||(deleted.y == range.maxY())||(deleted.z == range.maxZ()) || (deleted.x == range.minX())||(deleted.y == range.minY())||(deleted.z == range.minZ())  ){
				updateRange();
			}
			
			return true;
		} else {
			System.out.println("!!! kPointCloud.removeAt("+index+") index out of range");
			return false;
		}
	}
	
	public boolean removeAt(int[] indices) {
		// TODO: update the range based on this removal... this ruins the range's validity!!!!!!!!!!!!
			kVec[] tempPts = new kVec[points.length - indices.length];
			int m = 0;
			for (int n = 0; n < points.length; n++) {
				boolean removeIt = false;
				for (int i = 0; i < indices.length; i++) {
					if (n==i){
						removeIt = true;
						break;
					}
				}
				if (!removeIt) {
					tempPts[m] = points[n];
					m++;
				}
			}
			points = tempPts;
			
			return true;
	}
	

	/**
	 * inserts a clone of a given point into this cloud
	 */
	public int append(kVec p) {
		int i = indexOfVerySimilar(p, thresh);
		
		if (i==-1) {
			kVec[] tempPts = new kVec[points.length + 1];
			for (int n = 0; n < points.length; n++) {
				tempPts[n] = points[n];
			}
			tempPts[points.length] = kVec.clone(p);
			if (points.length == 0) range = new kRange(p.x, p.x, p.y, p.y, p.z, p.z);
			else range.expandToPoint(p);
			points = tempPts;
			return points.length-1;
			// System.out.println("inserting point at (" + p.x + "," + p.y + "," + p.z+ ")");
		} else {
			return i;
			// System.out.println("warning - duplicate point found at (" + p.x + "," + p.y + "," + p.z+ ")");
		}
	}

	/**
	 * inserts a point into this cloud at a given position
	 */
	public int append(float x, float y, float z) {
		return append(new kVec(x, y, z));
	}

	public void append( kPointCloud pCloud ) {
		for (int n = 0; n < pCloud.points.length; n++) {
			kVec[] temp = new kVec[points.length + 1];
			for (int i = 0; i < points.length; i++) temp[i] = points[i];
			temp[points.length] = pCloud.points[n];
			points = temp;
		}
		updateRange();
	}

	public void readPtsFromURL(URL url, float thresh) {
		try {
			BufferedReader infile = new BufferedReader(new InputStreamReader(url.openStream()));
			String str;
			int n = 0;
			int dup = 0;
			while ((str = infile.readLine()) != null) {
				// System.out.println(str);
				String[] strArr = str.split(",");
				kVec p = new kVec(Float.parseFloat(strArr[0]), Float.parseFloat(strArr[1]), Float.parseFloat(strArr[2]));
				if (containsVerySimilar(p, thresh)) dup++;
				else {
					append(p);
					n++;
				}

			}
			System.out.println("imported " + n + " points from file " + url);
			System.out.println(dup + " duplicate points found " + url);
			infile.close();
		} catch (IOException e) {
			System.out.println("Exception: " + e.getMessage() + "has occurred");
		}

	}

	/*
	 * SECONDARY PUBLIC METHODS
	 */

	public void updateRange(){
		if (points.length>0){
			range = new kRange(points[0]);
			for (int n = 0; n < points.length; n++) {
				range.expandToPoint(points[n]);
			}
		} else {
			range = new kRange();
		}
	}
	
	
	/**
	 * returns the minimum distance between points in the cloud
	 * 
	 * @return
	 */
	public float minDist() {
		// TODO: find a less costly algo for this...
		if (points.length > 2) {
			double minDist = points[0].distance(points[1]);
			for (int n = 0; n < points.length; n++) {
				// for every point in the cloud, compare to every other in the list after
				for (int m = n + 1; m < points.length; m++) {
					double dist = points[n].distance(points[m]);
					if ((dist < minDist) && (dist > 0.0f)) {
						// System.out.println("new minDist = " + points[n].distance(points[m]) + " btwn point["+n+"] & point["+m+"]");
						// System.out.println("point "+n+" at (" + points[n].x + "," + points[n].y + "," + points[n].z + ")");
						// System.out.println("point "+m+" at (" + points[m].x + "," + points[m].y + "," + points[m].z + ")");
						minDist = dist;
					}
				}
			}
			return (float) minDist;
		} else {
			return -1;
		}

	}

	/**
	 * translates the entire cloud by a given vector
	 * 
	 * @param vec
	 */
	public void translate(kVec vec) {
		for (int n = 0; n < points.length; n++) {
			points[n].translate(vec);
		}
		updateRange();
	}

	/**
	 * scales the entire cloud by a given factor
	 * 
	 * @param s
	 */
	public void scale(float s) {range.scale(s);for (int n = 0; n < points.length; n++) points[n].scale(s); updateRange();}
    public void scaleX(float s){range.scaleX(s);for (int n = 0; n < points.length; n++) points[n].scaleX(s); updateRange();}
    public void scaleY(float s){range.scaleY(s);for (int n = 0; n < points.length; n++) points[n].scaleY(s); updateRange();}
    public void scaleZ(float s){range.scaleZ(s);for (int n = 0; n < points.length; n++) points[n].scaleZ(s); updateRange();}

	
	public boolean containsVerySimilar(kVec p, float thresh) {
		return indexOfVerySimilar(p,thresh)>=0;
	}

	public int indexOfVerySimilar(kVec p, float thresh) {
		if (points.length > 0) {
			for (int n = 0; n < points.length; n++) {
				if (points[n].isIdentical(p, thresh)) return n;
			}
		}
		return -1;
	}	

	public float getThresh() {
		return thresh;
	}

	public void setThresh(float thresh) {
		this.thresh = thresh;
		//TODO: check for duplicates if thresh is lower than previous
	}
	
	
	public kVec centroid(){
		if (points.length == 0){
			System.out.println("!!! kPointCloud.centroid(): cloud empty");
			return new kVec();
		}
		return kVec.centroid(points);
	}
	
	
	public void setPtsX(float x){ for (int n = 0; n < points.length; n++) points[n].x = x; updateRange();}
	public void setPtsY(float y){ for (int n = 0; n < points.length; n++) points[n].y = y; updateRange();}
	public void setPtsZ(float z){ for (int n = 0; n < points.length; n++) points[n].z = z; updateRange();}	
	
	public static kPointCloud flipAxis(kPointCloud pCloud, int axisFlip){
		kPointCloud ret = new kPointCloud();
		for (int n = 0; n < pCloud.points.length; n++) ret.append(kVec.flipAxis(pCloud.points[n],axisFlip));
		ret.updateRange();
		return ret;
	}
	
	/**
	 * @param pCloud
	 * 		an existing point cloud to slice
	 * @param res
	 * 		the number of slices to take
	 * @param planeIndex
	 * 		the plane to slice about: 0=xz, 1=xy , 2=yz
	 * @return 
	 * 		an array of point clouds, each a "slice" of the given cloud about an orthagonal plane
	 */
	public static kPointCloud[] sliceup (kPointCloud pCloud, int res, int planeIndex){
		kPointCloud[] ret = new kPointCloud[0];
		float step, minOff;	
		step = (pCloud.range.deltY()/res);
		minOff = pCloud.range.minY();
		for (int m = 0; m < pCloud.points.length; m++){
			switch (planeIndex){
			case 1:step = (pCloud.range.deltZ()/res); minOff=pCloud.range.minZ(); break;			
			case 2:step = (pCloud.range.deltX()/res); minOff=pCloud.range.minX(); break;
			}
		}
		for (int n = 0; n < res+1; n++){
			kPointCloud slice = slice(pCloud, planeIndex, (n*step)+minOff, ((n+1)*step)+minOff);
			if (slice.count() > 0){
				kPointCloud[] temp = new kPointCloud[ret.length + 1];
				for (int m = 0; m < ret.length; m++) temp[m] = ret[m];
				temp[ret.length] = slice;
				ret = temp;
			}
			
		}
		

		return ret;
	}
	/**
	 * @param pCloud
	 * 		an existing point cloud to slice
	 * @param planeIndex
	 * 		the plane to slice about: 0=xz, 1=xy , 2=yz
	 * @param minVal
	 * 		min boundary of slice
	 * @param maxVal
	 * 		max boundary of slice
	 * @return 
	 * 		a new point cloud, containing a "slice" of the given cloud about an orthagonal plane
	 */
	public static kPointCloud slice(kPointCloud pCloud, int planeIndex, float minVal, float maxVal) {
		kPointCloud slice = new kPointCloud();
		for (int m = 0; m < pCloud.points.length; m++){
			switch (planeIndex){
			case 1:
				if ( (pCloud.points[m].z >= minVal )&&(pCloud.points[m].z < maxVal )){slice.append(pCloud.getAt(m));}
				break;			
			case 2:
				if ( (pCloud.points[m].x >= minVal )&&(pCloud.points[m].x < maxVal )){slice.append(pCloud.getAt(m));}
				break;
			default:
				if ( (pCloud.points[m].y >= minVal )&&(pCloud.points[m].y < maxVal )){slice.append(pCloud.getAt(m));}
				break;
			}
		}
		return slice;
	}
	/**
	 * @param pCloud
	 * 		an existing point cloud to slice
	 * @param planeIndex
	 * 		the plane to slice about: 0=xz, 1=xy , 2=yz
	 * @param sliceCenter
	 * 		the center point of a slice to take
	 * @param sliceThickness
	 * 		the thickness of a slice to take
	 * @return
	 */
	public static kPointCloud slice(kPointCloud pCloud, int planeIndex, kVec sliceCenter, float sliceThickness) {
		float minVal = sliceCenter.y - (sliceThickness/2);
		float maxVal = sliceCenter.y + (sliceThickness/2);
		switch (planeIndex){
		case 1: minVal = sliceCenter.z - (sliceThickness/2); maxVal = sliceCenter.z + (sliceThickness/2); break;			
		case 2: minVal = sliceCenter.x - (sliceThickness/2); maxVal = sliceCenter.x + (sliceThickness/2); break;
		}
		return kPointCloud.slice( pCloud, planeIndex, minVal, maxVal);
	}
	
	
	// PORTED FROM POINT CLASS

	/**
	 * Returns all points in a given array which lie within a given distance d from this point AND WHICH ARE NOT ONTOP OF this point
	 * 
	 * @param pArr
	 * @param d
	 * @return
	 */
	/*
	TODO: complete getNearestPoint method
	public int[] getIndicesOfNearPoints(kPoint[] pArr, double d) {
		int[] returnArr = new int[0];
		for (int i = 0; i < pArr.length; i++) {
			if ((this.distance(pArr[i]) < d) && (this.distance(pArr[i]) != 0.0)) {
				int[] temp = new int[returnArr.length + 1];
				temp[returnArr.length] = i;
				System.arraycopy(returnArr, 0, temp, 0, returnArr.length);
				returnArr = temp;
			}
		}
		return returnArr;
	}
	 */

	/**
	 * @param pArr
	 * @return
	 */
	/*
	TODO: complete getNearestPoint method
	public kPoint getNearestPoint(kPoint[] pArr) {
		// returns the nearest point which is NOT ONTOP of this point
		kPoint returnPt = pArr[0];
		double d = this.distance(returnPt);
		// returns the nearest point to this point
		for (int i = 1; i < pArr.length; i++) {
			if ((this.distance(pArr[i]) < d) && (this.distance(pArr[i]) != 0.0)) {
				returnPt = pArr[i];
			}
		}
		return kPoint.clone(returnPt);

	}
	 */

	/**
	 * returns the index of a point in this cloud which is nearest to a given point
	 * @param pt
	 * @param includeIdentical
	 * @return
	 */
	
	public int getIndexOfNearestPoint(kVec pt, boolean includeIdentical) {
		int returnPt = -1;
		double d = -10.0f;
		for (int i = 0; i < points.length; i++) {
			float dist = pt.distance(points[i]);
			if ((d==-10.0f)||( dist < d )){
				returnPt = i;
				if ((includeIdentical)||(d>kElement.tolerence)){
					returnPt = i;
				}
			}
		}
		return returnPt;
	} 
	 
	
	public String toString(){
		return "pointcloud ["+points.length+" points]";
	}
	
	public String toPointString(){
		String ret = "";
		for (int i = 0; i < points.length; i++) {
			ret += points[i].toString() + " ";
		}
		return ret;
	}

}
